from Scientific.Geometry.Objects3D import Circle
from Scientific.Statistics import mean
from Scientific.Geometry import Vector
from datetime import datetime, timedelta
from decimal import Decimal
from math import cos, acos, sin, asin, tan, atan2
from math import pi, degrees, radians
from math import sqrt, pow

def pprint(v, places):
	"""
	Function to pretty print a vector, with a maximum number
	of decimal places.
	>>> pprint(Vector(0.123456, 0.789012, 0.345678), 4)
	[0.1235, 0.7890, 0.3457]
	"""
	fmt = "[%%.0%sf, %%.0%sf, %%.0%sf]" % (places, places, places)
	print fmt % (v[0], v[1], v[2])

def earth_radius(long, lat):
	return 6371001
	
def earth_omega():
	"""
	the angular speed of the earth in terms of radians / second
	"""
	return - 2*pi / (24 * 60 * 60)

def cartesian(v):
	"""
	Transform a vector in geographic sphereical coordinates,
	(longitude, latitude, altitude) to a cartesian coordinate
	system, centered at the centre of mass of the earth, with
	the z-axis pointing to the north pole, and the x-axis 
	pointing towards the Greenwich meridian.
	>>> tests = [
	...	Vector(0,0,1),
	...	Vector(90,0,1),
	...	Vector(0,90,1),
	...	Vector(-90,0,1),
	... ]
	>>> for v in tests:
	...	pprint(cartesian(v), 4)
	...
	[1.0000, 0.0000, 0.0000]
	[0.0000, 1.0000, 0.0000]
	[0.0000, 0.0000, 1.0000]
	[0.0000, -1.0000, 0.0000]
	"""
	phi = radians(v[0])
	theta = radians(90 - v[1])
	rho = v[2]
	x = rho * sin(theta) * cos(phi)
	y = rho * sin(theta) * sin(phi)
	z = rho * cos(theta)
	return Vector(x, y, z)

def geographic(v):
	"""
	>>> tests = [
	...	Vector(1,0,0),
	...	Vector(0,1,0),
	...	Vector(0,0,1),
	...	Vector(0,-1,0),
	... ]
	>>> for v in tests:
	...	pprint(geographic(v), 4)
	...
	[0.0000, 0.0000, 1.0000]
	[90.0000, 0.0000, 1.0000]
	[0.0000, 90.0000, 1.0000]
	[-90.0000, 0.0000, 1.0000]
	"""
	x = v[0]
	y = v[1]
	z = v[2]
	## longitude
	if x == 0 and y == 0:
		phi = 0
	else:
		phi = atan2(y,x) #acos( x / sqrt( pow(x,2) + pow(y,2) ) )
	## latitude
	if x == 0 and y == 0 and z == 0:
		theta = 0
	else:
		theta = acos( z / sqrt( pow(x,2) + pow(y,2) + pow(z,2) ) )
	## altitude
	rho = sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2))
	return Vector(degrees(phi), 90 - degrees(theta), rho)

class Orbit(Circle):
	"""
	Implement the orbital path and swath of a satellite given
	the plane and radius of the orbit, the centre of mass of
	the system, and angular velocity, and the initial conditions.

	Assumptions:

		1. The orbit is a circle. The more eccentric the
		   orbit the less accurate this approximation
		2. The orbital plane is stable does not precess.
		3. The satellite is small -- the centre of mass
		   of the system and that of the earth are the
		   same.

	>>> r_s = Vector(-120.71524, 81.088751, 6371001)
	>>> t_s = datetime(2004, 9, 1, 1, 13, 38)
	>>> r_f = Vector(103.575242, -63.080187, 6371001)
	>>> t_f = datetime(2004, 9, 1, 1, 57, 27)
	>>> o = Orbit.new(r_s, t_s, r_f, t_f)
	>>> pprint(geographic(o.position(t_s)), 4)
	[-120.7152, 81.0888, 6371001.0000]
	>>> pprint(geographic(o.position(t_f)), 4)
	[114.5294, -63.0802, 6371001.0000]
	>>> for r in o.path(t_s, t_f):
	...	pprint(r, 4)
	...
	[-120.7152, 81.0888, 6371001.0000]
	[163.1456, 74.4967, 6371001.0000]
	[144.5536, 60.1523, 6371001.0000]
	[136.9419, 44.9369, 6371001.0000]
	[132.1268, 29.5093, 6371001.0000]
	[128.3194, 14.0060, 6371001.0000]
	[124.8637, -1.5235, 6371001.0000]
	[121.3727, -17.0505, 6371001.0000]
	[117.4355, -32.5440, 6371001.0000]
	[112.2810, -47.9460, 6371001.0000]
	[103.6090, -63.0802, 6371001.0000]
	"""
	def __init__(self, x_0, normal, radius, t_0, omega):
		Circle.__init__(self, Vector(0,0,0), normal, radius)
		self.omega = omega
		self.x_0 = x_0
		self.y_0 = x_0.cross(normal)
		self.t_0 = t_0

	@classmethod
	def new(self, r_s, t_s, r_f, t_f):
		###
		### given two endpoints  in the earth's coordinate system,
		### construct an orbit
		###
		### 1. how much has the earth rotated in this time? (radians)
		dt = t_f - t_s
		dtheta = (dt.seconds + 0.0000001 * dt.microseconds) * earth_omega()
		### 2. transform the ending position to the starting frame of
		###    reference.
		r_f_0 = Vector(r_f[0] - degrees(dtheta), r_f[1], r_f[2])
		### 3. transform the starting and ending position vectors to
		###    cartesian coordinates for easy calculation.
		x_s = cartesian(r_s)
		x_f = cartesian(r_f_0)
		### 4. calculate the normal to the orbital plane.
		normal = x_f.normal().cross(x_s.normal()).normal()
		### 5. calculate the angular speed in the orbital plane.
		omega = x_f.angle(x_s) / dt.seconds
		### 6. Instantiate the orbit object.
		return Orbit(x_s, normal, r_s[2], t_s, omega)

	def position(self, t):
		###
		### Given a time, calculate the position vectors
		###
		### 1. time since t_0. datetime doesn't do negative times well
		###  
		dt = abs(t - self.t_0)
		if t < self.t_0:
			sign = -1
		else:
			sign = 1
		### 2. rotation (radians) in the orbital plane
		theta = sign * (dt.seconds + 0.000001 * dt.microseconds) * self.omega
		### 3. calculate the direction of the new position vector
		direction = cos(theta) * self.x_0 + sin(theta) * self.y_0
		### 4. normalize and multiply by the radius.
		position = direction.normal() * self.radius
		return position

	def velocity(self, position, t, dt=10):
		###
		### Given a position and a time, approximate the
		### instantaneous velocity vector
		###
		### set:
		###       p_0 < p < p_1
		###       t_0 + dt/2 = t = t_1 - dt/2
		###
		### 1. find the enclosing position vectors
		if not isinstance(dt, timedelta):
			dt = timedelta(0, dt)
		p_0 = self.position(t - dt/2)
		p_1 = self.position(t + dt/2)
		### 2. subtract to find the change in position
		dp = (p_1 - p_0)
		### 3. divide to find the velocity
		v = dp / (dt.seconds + 0.000001 * dt.microseconds)
		return v

	def path(self, t_0, t_n, steps = 10):
		###
		### Produce a sequence of steps + 1 position vectors
		### equally spaced in time from t_0 to t_n.
		###
		### 1. time per step, dt
		dt = (t_n - t_0) / steps
		for i in range(steps + 1):
			### 2. get the position at that time
			x = self.position(t_0 + i * dt)
			### 3. transform the position to the geographic
			###    coordinate system at t_0
			r_0 = geographic(x)
			### 4. transform to the current geographical coordinate
			###    system at t_0 + i * dt
			dtheta = i * (dt.seconds + 0.0000001 * dt.microseconds) * earth_omega()
			r = Vector(r_0[0] + degrees(dtheta), r_0[1], r_0[2])
			yield r

	def setSwath(self, l_0, r_0):
		###
		### given the left and right edges of the swath, calculate
		### and save its width
		###
		self.__swath__ = (cartesian(r_0) - cartesian(l_0)).length()

	def swath(self, t_0, t_n, steps = 10):
		###
		### produce a sequence of points, following the left edge
		### of the swath from t_0 to t_n, and then back up the
		### right edge from t_n back to t_0. This is the perimeter
		### of the swath.
		###
		left = []
		right = []
		### 1. time per step, dt
		dt = (t_n - t_0) / steps
		for i in range(steps + 1):
			### 2. calculate current time, and angle (in the plane of orbit)
			t = t_0 + i * dt
			dtheta = i * (dt.seconds + 0.0000001 * dt.microseconds) * earth_omega()

			### 3. get the current position and velocity, and the normal to
			###    the plane of orbit
			x = self.position(t)
			v = self.velocity(x, t)
			### this could be simpler, could just reuse normal.
			n = v.cross(x).normal()

			### 4. calculate left and right edges of the swath
			x_left = x - n * self.__swath__ / 2
			x_right = x + n * self.__swath__ / 2

			### 5. transform points to geographic coordinate system at t_0
			r_left_0 = geographic(x_left)
			r_0 = geographic(x)
			r_right_0 = geographic(x_right)

			### 6. transform points to geographic coordinate system at current
			###    time
			r_left = Vector(r_left_0[0] + degrees(dtheta), r_left_0[1], r_left_0[2])
			r = Vector(r_0[0] + degrees(dtheta), r_0[1], r_0[2])
			r_right = Vector(r_right_0[0] + degrees(dtheta), r_right_0[1], r_right_0[2])

			### save
			left.append(r_left)
			right.append(r_right)

		### 7 construct the perimeter, in order.
		right.reverse()
		polygon = left + right + [left[0]]
		return polygon

if __name__ == '__main__': 
	from sys import argv, exit
	### run doctests
	import doctest
	doctest.testmod()
